#ifndef WALKAWARE_SERVICES_PLANEDETECTIONSERVICE_H
#define WALKAWARE_SERVICES_PLANEDETECTIONSERVICE_H

#include <QMutexLocker>

#include <WalkaWare/Shared/PlaneDetector.h>
#include <WalkaWare/Shared/Vec3.h>
#include <WalkaWare/Services/PollingService.h>

namespace WalkaWare
{

namespace Services
{

class PlaneDetectionService : public PollingService
{
	Q_OBJECT

	public:

		typedef PollingService        BaseType;
		typedef PlaneDetectionService ThisType;

		PlaneDetectionService(void)
			: inTimestamp       (0)
			, inPreferredNormal (Shared::Vec3f(0.0f, 1.0f, 0.0f).Normalize())
			, inReady           (false)
		{
			;
		}

		virtual ~PlaneDetectionService(void)
		{
			this->quit();
		}

	signals:

		void planeReady(float offset, float normalX, float normalY, float normalZ, quint32 timestamp);

	public slots:

		void detect(WalkaWare::Shared::PointCloud pointCloud, quint32 timestamp)
		{
			if (!this->isRunning()) return;
			this->pushPointCloud(pointCloud, timestamp);
		}

		void setAcceleration(float x, float y, float z, quint32 timestamp)
		{
			(void)timestamp;

			if (!this->isRunning()) return;

			{
				QMutexLocker(&(this->sMutex));
				this->inPreferredNormal = Shared::Vec3f(x, y, z).Normalize();
			}
		}

	protected:

		Shared::PlaneDetector detector;
		quint32               inTimestamp;
		Shared::PointCloud    inPointCloud;
		Shared::Vec3f         inPreferredNormal;
		bool                  inReady;

		void pushPointCloud(const Shared::PointCloud & pointCloud, quint32 timestamp)
		{
			{
				QMutexLocker(&(this->sMutex));
				if (this->end) return;
				this->inPointCloud = pointCloud;
				this->inTimestamp  = timestamp;
				this->inReady      = true;
			}
			this->sCondition.wakeAll();
		}

		bool popPointCloud(Shared::PointCloud & pointCloud, quint32 & timestamp)
		{
			bool ended = false;
			this->sMutex.lock();
			while (!this->end && !this->inReady)
			{
				this->sCondition.wait(&(this->sMutex));
			}
			ended = this->end;
			if (!ended)
			{
				pointCloud = this->inPointCloud;
				timestamp  = this->inTimestamp;
				this->inReady = false;
			}
			this->sMutex.unlock();
			return !ended;
		}

		virtual void run(void)
		{
			const WalkaWare::Shared::Box3f bbox(WalkaWare::Shared::Vec3f(-0.5f, -1.5f, -2.5f), WalkaWare::Shared::Vec3f(0.5f, -0.5f, -1.5f));

			Shared::PointCloud pointCloud;
			quint32            timestamp = 0;

			while (this->popPointCloud(pointCloud, timestamp))
			{
				Shared::Plane3f plane;
				float           error = 0.0f;
				if (!this->detector.detect(pointCloud, bbox, this->inPreferredNormal, plane, error)) continue;
				emit planeReady(plane.Offset(), plane.Direction()[0], plane.Direction()[1], plane.Direction()[2], timestamp);
			}
		}

	private:

		Q_DISABLE_COPY(PlaneDetectionService)
};

} // end namespace Services

} // end namespace WalkaWare

#endif // WALKAWARE_SERVICES_PLANEDETECTIONSERVICE_H
